#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

typedef struct {
    float x;  // 状态变量
    float P;  // 状态协方差
    float Q;  // 系统噪声协方差
    float R;  // 测量噪声协方差
    float K;  // 卡尔曼增益
} KalmanFilter;

void initKalmanFilter(KalmanFilter *kf, float initial_x, float initial_P, float Q, float R) {
    kf->x = initial_x;
    kf->P = initial_P;
    kf->Q = Q;
    kf->R = R;
    kf->K = 0;
}

void updateKalmanFilter(KalmanFilter *kf, float measurement) {
    kf->x = kf->x;
    kf->P = kf->P + kf->Q;

    // 根据测量与当前估计的差异来调整卡尔曼增益，抑制较大干扰
    if (fabs(measurement - kf->x) > 10.0) {
        kf->K = kf->P / (kf->P + kf->R * 10);  // 增大测量噪声对卡尔曼增益的影响
    } else {
        kf->K = kf->P / (kf->P + kf->R);
    }

    kf->x = kf->x + kf->K * (measurement - kf->x);
    kf->P = (1 - kf->K) * kf->P;
}

int main() {
    // 设置随机数种子
    srand(time(NULL));

    KalmanFilter kf;
    initKalmanFilter(&kf, 0, 1, 0.01, 0.1);

    // 生成随机噪声信号的示例输入数组
    float inputArray[100];
    for (int i = 0; i < 100; ++i) {
        // 生成范围在 -1 到 1 之间的随机噪声
        float noise = ((float)rand() / RAND_MAX) * 2.0 - 1.0;

        // 模拟输入，添加随机噪声
        float measurement = 2.0 + i * 0.1 + noise;

        // 处理第31和第89位置的较大干扰
        if (i == 31) {
            measurement += 20;
        }

        if (i == 86) {
            measurement -= 20;
        }

        if (i == 87) {
            measurement -= 20;
        }

        if (i == 88) {
            measurement -= 20;
        }

        if (i == 89) {
            measurement -= 20;
        }

        // 使用卡尔曼滤波器进行滤波
        updateKalmanFilter(&kf, measurement);

        // 输出每秒的滤波结果
        printf("%.4f,%.4f\n", measurement, kf.x);
    }

    return 0;
}
